#include "precom.h"
#include "Core.h"

using namespace fmath;

Core::Core()
{
	m_bumpCoef = 0.001f;
	m_toolList = 0;
	m_radius = 0.05f;
	m_step = 0.06f;
	m_toolPos.m_point.set(0.f, 0.f, 0.f);
	m_toolPos.m_eulerAngles.set(0.f, 0.f);
	//CompileToolList(m_radius);
	m_indX = m_indY = m_indZ = 0;
	m_toolAxis = GetToolAxisVector();
}

void Core::DrawStationaryDistance()
{
	fmath::vector3 point;
	std::list<vector3>::const_iterator citer;
	citer = m_stationaryPoints.begin();
	while(citer != m_stationaryPoints.end() ) {
		point = *citer;
		glVertex3f(citer->x, citer->y, citer->z);
		++citer;
		glVertex3f(citer->x, citer->y, citer->z);
		++citer;
		glVertex3f(point.x, point.y, point.z);
		point += *citer;
		glVertex3f(point.x, point.y, point.z);
		++citer;
	}
}

void Core::DrawRaysFromTool(unsigned long a_triangle, const Mesh* a_mesh) const
{
	unsigned long ind[3];
	a_mesh->GetTriangleVertexes(a_triangle, ind);
	vector3 v;

	v = a_mesh->m_vertices[ind[0]].m_position;
	v -= m_toolPos.m_point;
	float dot = DotProduct(v, m_toolAxis);
	if(dot < 0.f) v = m_toolPos.m_point;
	else v = m_toolPos.m_point + (dot * m_toolAxis);
	glVertex3f(v.x, v.y, v.z);
	glVertex3fv(a_mesh->m_vertices[ind[0]].m_position);

	v = a_mesh->m_vertices[ind[1]].m_position;
	v -= m_toolPos.m_point;
	dot = DotProduct(v, m_toolAxis);
	if(dot < 0.f) v = m_toolPos.m_point;
	else v = m_toolPos.m_point + (dot * m_toolAxis);
	glVertex3f(v.x, v.y, v.z);
	glVertex3fv(a_mesh->m_vertices[ind[1]].m_position);

	v = a_mesh->m_vertices[ind[2]].m_position;
	v -= m_toolPos.m_point;
	dot = DotProduct(v, m_toolAxis);
	if(dot < 0.f) v = m_toolPos.m_point;
	else v = m_toolPos.m_point + (dot * m_toolAxis);
	glVertex3f(v.x, v.y, v.z);
	glVertex3fv(a_mesh->m_vertices[ind[2]].m_position);
}

void Core::DrawRaysToCenter(unsigned long a_triangle, const Mesh* a_mesh) const
{
	unsigned long ind[3];
	a_mesh->GetTriangleVertexes(a_triangle, ind);

	glVertex3f(0.f, a_mesh->m_vertices[ind[0]].m_position[1], 0.f);
	glVertex3fv(a_mesh->m_vertices[ind[0]].m_position);
	glVertex3f(0.f, a_mesh->m_vertices[ind[1]].m_position[1], 0.f);
	glVertex3fv(a_mesh->m_vertices[ind[1]].m_position);
	glVertex3f(0.f, a_mesh->m_vertices[ind[2]].m_position[1], 0.f);
	glVertex3fv(a_mesh->m_vertices[ind[2]].m_position);
}

void Core::DrawTriangleWithLines(unsigned long a_triangle, const Mesh* a_mesh) const
{
	unsigned long ind[3];
	a_mesh->GetTriangleVertexes(a_triangle, ind);

	glVertex3fv(a_mesh->m_vertices[ind[0]].m_position);
	glVertex3fv(a_mesh->m_vertices[ind[1]].m_position);
	glVertex3fv(a_mesh->m_vertices[ind[1]].m_position);
	glVertex3fv(a_mesh->m_vertices[ind[2]].m_position);
	glVertex3fv(a_mesh->m_vertices[ind[2]].m_position);
	glVertex3fv(a_mesh->m_vertices[ind[0]].m_position);
}

void Core::DrawBumpTriangle(unsigned long a_triangle, const Mesh* a_mesh) const
{
	vector3 v1, v2, v3, n;
	unsigned long ind[3];
	a_mesh->GetTriangleVertexes(a_triangle, ind);

	v1 = a_mesh->m_vertices[ind[0]].m_position;
	n = a_mesh->m_normals[a_mesh->m_triangles[a_triangle].m_normals[0]].m_normal;
	v1 += n * m_bumpCoef;
	v2 = a_mesh->m_vertices[ind[1]].m_position;
	n = a_mesh->m_normals[a_mesh->m_triangles[a_triangle].m_normals[1]].m_normal;
	v2 += n * m_bumpCoef;
	v3 = a_mesh->m_vertices[ind[2]].m_position;
	n = a_mesh->m_normals[a_mesh->m_triangles[a_triangle].m_normals[2]].m_normal;
	v3 += n * m_bumpCoef;

	glVertex3f(v1.x, v1.y, v1.z);
	glVertex3f(v2.x, v2.y, v2.z);
	glVertex3f(v2.x, v2.y, v2.z);
	glVertex3f(v3.x, v3.y, v3.z);
	glVertex3f(v3.x, v3.y, v3.z);
	glVertex3f(v1.x, v1.y, v1.z);
}


void Core::DrawTool() const
{
	Box bbox = BoundTool();
	vector3 ax = m_toolAxis;
	ax *= m_model.GetBBox().GetMaxSize();
	ax += m_toolPos.m_point;

	glPushMatrix();
	glBegin(GL_LINES);
		glColor3f(0.f, 1.0f, 0.7f);

		DrawBoxUsingLines(bbox.m_min[0], bbox.m_min[1], bbox.m_min[2],
			bbox.m_max[0], bbox.m_max[1], bbox.m_max[2]);

		glVertex3f(m_toolPos.m_point.x, m_toolPos.m_point.y, m_toolPos.m_point.z);
		glVertex3f(ax.x, ax.y, ax.z);

	glEnd();
	glPopMatrix();
}

void Core::DrawCubeAndTriangles(bool a_raysToCenter)
{
	std::list<const CubeElement*>::const_iterator citer, cend;
	//std::list<CubeElement>::iterator iter, end;

	//citer = m_space.GetTriangleListFromCube(m_indX, m_indY, m_indZ).begin();
	//cend = m_space.GetTriangleListFromCube(m_indX, m_indY, m_indZ).end();
	glBegin(GL_LINES);
	/*glColor3f(0.5f, 1.0f, 1.0f);
	m_space.DrawCubeWithLines(m_indX, m_indY, m_indZ);
	iter = m_space.GetTriangleListFromCube(m_indX, m_indY, m_indZ).begin();
	end = m_space.GetTriangleListFromCube(m_indX, m_indY, m_indZ).end();
	while(iter != end) {
		glColor3f(1.f, 1.0f, 0.8f);
		DrawTriangleWithLines(iter->m_triangleIndex, iter->m_mesh);
		++iter;
	}*/
	glColor3f(1.f, 1.f, 0.1f);
	DrawStationaryDistance();

	if(m_showList1) {
		citer = m_triangleToCompute.begin();
		cend = m_triangleToCompute.end();
		while(citer != cend) {
			glColor3f(1.f, 0.0f, 0.f);
			DrawBumpTriangle((*citer)->m_triangleIndex, (*citer)->m_mesh);
			if(m_showRays) {
				glColor3f(0.9f, 0.6f, 0.0f);
				DrawRaysFromTool((*citer)->m_triangleIndex, (*citer)->m_mesh);
			}
			++citer;
		}
	}
	if(m_showList2) {
		citer = m_triangleToCompute2.begin();
		cend = m_triangleToCompute2.end();
		while(citer != cend) {
			glColor3f(1.f, 0.0f, 0.f);
			DrawBumpTriangle((*citer)->m_triangleIndex, (*citer)->m_mesh);
			if(m_showRays) {
				glColor3f(0.9f, 0.6f, 0.0f);
				DrawRaysFromTool((*citer)->m_triangleIndex, (*citer)->m_mesh);
			}
			++citer;
		}
	}
	glEnd();
}

void Core::Clear()
{
	ClearLists();
	if(m_toolList) {
		glDeleteLists(m_toolList, 1);
		m_toolList = 0;
	}
}

void Core::CompileToolList(float a_radius)
{
	if(m_toolList) {
		glDeleteLists(m_toolList, 1);
		m_toolList = 0;
	}
	m_toolList = m_sphereGenerator.GenerateSphere(a_radius, 
		m_model.GetBBox().GetMaxSize() * 1.5f);
}

void Core::AddTriangleToList(const CubeElement* a_elem, std::list<const CubeElement*>& o_list) const
{
	std::list<const CubeElement*>::const_iterator iter = o_list.begin();
	while(iter != o_list.end()) {
		if(( (*iter)->m_triangleIndex == a_elem->m_triangleIndex )
			&& ( (*iter)->m_mesh == a_elem->m_mesh ) ) return;
		++iter;
	}
	o_list.push_back(a_elem);
}

bool Core::FindTrianglesFacedToTool()
{
	std::list<const CubeElement*> triangles;
	std::list<const CubeElement*>::const_iterator citer;

	m_space.GetCollideCubes(BoundTool(), triangles);
	
	citer = triangles.begin();
	while(citer != triangles.end()) {
		if(IsFacedToTool(*citer))
			AddTriangleToList(*citer, m_triangleToCompute);
		if(IsFacedToTool2(*citer))
			AddTriangleToList(*citer, m_triangleToCompute2);
		//m_triangleToCompute.push_back(*citer);
		++citer;
	}

	return true;
}

bool Core::CompareVectors(const fmath::vector3 &a_cross1, const fmath::vector3 &a_cross2, const fmath::vector3 &a_cross3, const fmath::vector3& a_negligibleDir) const
{
	fmath::vector3 v1, v2, v3;
	v1 = a_cross1 - DotProduct(a_cross1, a_negligibleDir)*a_negligibleDir;
	v2 = a_cross2 - DotProduct(a_cross2, a_negligibleDir)*a_negligibleDir;
	v3 = a_cross3 - DotProduct(a_cross3, a_negligibleDir)*a_negligibleDir;
	bool isZeroX, isZeroY, isZeroZ, goodX, goodY, goodZ;
	
	isZeroX = (v1.x < EPS && v1.x >-EPS)
		|| (v2.x < EPS && v2.x >-EPS)
		|| (v3.x < EPS && v3.x >-EPS);
	isZeroY = (v1.y < EPS && v1.y >-EPS)
		|| (v2.y < EPS && v2.y >-EPS)
		|| (v3.y < EPS && v3.y >-EPS);
	isZeroZ = (v1.z < EPS && v1.z >-EPS)
		|| (v2.z < EPS && v2.z >-EPS)
		|| (v3.z < EPS && v3.z >-EPS);
	goodX = ( (v1.x * v2.x) < 0.f )
		|| ( (v1.x * v3.x) < 0.f )
		|| ( (v3.x * v2.x) < 0.f );
	goodY = ( (v1.y * v2.y) < 0.f )
		|| ( (v1.y * v3.y) < 0.f )
		|| ( (v3.y * v2.y) < 0.f );
	goodZ = ( (v1.z * v2.z) < 0.f )
		|| ( (v1.z * v3.z) < 0.f )
		|| ( (v3.z * v2.z) < 0.f );
	return (goodX || isZeroX) 
		&& (goodY || isZeroY)
		&& (goodZ || isZeroZ);
}


bool Core::CompareVectors(const fmath::vector3 &a_cross1, const fmath::vector3 &a_cross2, const fmath::vector3 &a_cross3) const
{
	bool isZeroX, isZeroY, isZeroZ, goodX, goodY, goodZ;
	isZeroX = (a_cross1.x < EPS && a_cross1.x >-EPS)
		|| (a_cross2.x < EPS && a_cross2.x >-EPS)
		|| (a_cross3.x < EPS && a_cross3.x >-EPS);
	isZeroY = (a_cross1.y < EPS && a_cross1.y >-EPS)
		|| (a_cross2.y < EPS && a_cross2.y >-EPS)
		|| (a_cross3.y < EPS && a_cross3.y >-EPS);
	isZeroZ = (a_cross1.z < EPS && a_cross1.z >-EPS)
		|| (a_cross2.z < EPS && a_cross2.z >-EPS)
		|| (a_cross3.z < EPS && a_cross3.z >-EPS);
	goodX = ( (a_cross1.x * a_cross2.x) < 0.f )
		|| ( (a_cross1.x * a_cross3.x) < 0.f )
		|| ( (a_cross3.x * a_cross2.x) < 0.f );
	goodY = ( (a_cross1.y * a_cross2.y) < 0.f )
		|| ( (a_cross1.y * a_cross3.y) < 0.f )
		|| ( (a_cross3.y * a_cross2.y) < 0.f );
	goodZ = ( (a_cross1.z * a_cross2.z) < 0.f )
		|| ( (a_cross1.z * a_cross3.z) < 0.f )
		|| ( (a_cross3.z * a_cross2.z) < 0.f );
	return (goodX || isZeroX) 
		&& (goodY || isZeroY)
		&& (goodZ || isZeroZ);
}

vector3 Core::GetToolNormalForPoint(const float* a_point) const
{
	vector3 ret = vector3(a_point) - m_toolPos.m_point;
	vector3 ax = m_toolAxis;
	float dot = DotProduct(ret, ax);
	if(dot < 0.f) {
		//ret = m_toolPos.m_point - ret;
		ret.normalize();
		return ret;
	}
	ax = (dot * ax) + m_toolPos.m_point;
	ret = vector3(a_point) - ax;
	ret.normalize();
	return ret;
}

bool Core::IsFacedToTool(const CubeElement *a_triangle) const
{
	vector3 n1, n2, n3, n;
	vector3 v1, v2, v3;
	unsigned long* ind;
	unsigned long vInd[3];

	ind = a_triangle->m_mesh->m_triangles[a_triangle->m_triangleIndex].m_normals;
	n1 = a_triangle->m_mesh->m_normals[ind[0]].m_normal;
	vInd[0] = a_triangle->m_mesh->m_normals[ind[0]].m_vertexNum;
	n2 = a_triangle->m_mesh->m_normals[ind[1]].m_normal;
	vInd[1] = a_triangle->m_mesh->m_normals[ind[1]].m_vertexNum;
	n3 = a_triangle->m_mesh->m_normals[ind[2]].m_normal;
	vInd[2] = a_triangle->m_mesh->m_normals[ind[2]].m_vertexNum;
	
	n = n1 + n2 + n3; n.normalize();
	v1 = GetToolNormalForPoint(a_triangle->m_mesh->m_vertices[vInd[0]].m_position);
	v2 = GetToolNormalForPoint(a_triangle->m_mesh->m_vertices[vInd[1]].m_position);
	v3 = GetToolNormalForPoint(a_triangle->m_mesh->m_vertices[vInd[2]].m_position);
	
	return CompareVectors(n1 + v1, n2 + v2, n3 + v3, n);
}

bool Core::IsFacedToTool2(const CubeElement *a_triangle) const
{
	vector3 n1, n2, n3, n;
	vector3 v1, v2, v3;
	unsigned long* ind;
	unsigned long vInd[3];

	ind = a_triangle->m_mesh->m_triangles[a_triangle->m_triangleIndex].m_normals;
	n1 = a_triangle->m_mesh->m_normals[ind[0]].m_normal;
	vInd[0] = a_triangle->m_mesh->m_normals[ind[0]].m_vertexNum;
	n2 = a_triangle->m_mesh->m_normals[ind[1]].m_normal;
	vInd[1] = a_triangle->m_mesh->m_normals[ind[1]].m_vertexNum;
	n3 = a_triangle->m_mesh->m_normals[ind[2]].m_normal;
	vInd[2] = a_triangle->m_mesh->m_normals[ind[2]].m_vertexNum;

	n = n1 + n2 + n3; n.normalize(); 
	v1 = GetToolNormalForPoint(a_triangle->m_mesh->m_vertices[vInd[0]].m_position);
	v2 = GetToolNormalForPoint(a_triangle->m_mesh->m_vertices[vInd[1]].m_position);
	v3 = GetToolNormalForPoint(a_triangle->m_mesh->m_vertices[vInd[2]].m_position);
	
	return CompareVectors(CrossProduct(n1, v1), CrossProduct(n2, v2), CrossProduct(n3, v3), n);
}


void Core::CalculateVectorOnTangentPlane(const fmath::vector3& a_point, const fmath::vector3& a_norm, fmath::vector3& o_vec) const
{
	//matrix33 loc;
	vector3 n;
	//float x, y, z;

	n = GetToolNormalForPoint( &a_point.x );
	o_vec = a_norm + n;
	//z = sqrtf(o_vec.x * o_vec.x + o_vec.y * o_vec.y + o_vec.z * o_vec.z);
	//len = sqrtf(o_vec.x * o_vec.x + o_vec.y * o_vec.y);
	//x = atan( len/o_vec.z );
	//y = atan2(o_vec.y, o_vec.x);
	
	//loc = GetLocalTangentSpace(n);
	//x = DotProduct(o_vec, loc[0]);
	//y = DotProduct(o_vec, loc[1]);
	//z = DotProduct(o_vec, loc[2]);
	//o_vec.set(x-n.x, y-n.y, z-n.z);
}

void Core::FindStationaryPoints()
{
	std::list<const CubeElement*>::const_iterator citer;
	matrix33 Nt, Ns;
	Triangle tr;
	float dot;
	unsigned long ind[3];
	vector3 n, v1, v2, v3;

	m_stationaryPoints.clear();
	m_coords.clear();
	m_distances.clear();
	m_surfaceNormals.clear();

	citer = m_triangleToCompute2.begin();
	while( citer != m_triangleToCompute2.end() ) {
		tr = (*citer)->m_mesh->m_triangles[(*citer)->m_triangleIndex];
		(*citer)->m_mesh->GetTriangleVertexes((*citer)->m_triangleIndex, ind);

		v1 = (*citer)->m_mesh->m_vertices[ind[0]].m_position;
		v2 = (*citer)->m_mesh->m_vertices[ind[1]].m_position;
		v3 = (*citer)->m_mesh->m_vertices[ind[2]].m_position;
		Ns[0] = (*citer)->m_mesh->m_normals[tr.m_normals[0]].m_normal;
		Ns[1] = (*citer)->m_mesh->m_normals[tr.m_normals[1]].m_normal;
		Ns[2] = (*citer)->m_mesh->m_normals[tr.m_normals[2]].m_normal;
		Nt[0] = GetToolNormalForPoint((*citer)->m_mesh->m_vertices[ind[0]].m_position);
		Nt[1] = GetToolNormalForPoint((*citer)->m_mesh->m_vertices[ind[1]].m_position);
		Nt[2] = GetToolNormalForPoint((*citer)->m_mesh->m_vertices[ind[2]].m_position);
	
		if(m_minimizer.Min(Ns, Nt)) {
			n.x = m_minimizer.A();
			n.y = m_minimizer.B();
			n.z = 1.0f - n.x - n.y;
			if(n.x < -EPS || n.y < -EPS || n.z < -EPS) {
				++citer;
				continue;
			}
			m_coords.push_back(n);
			v1 *= n.x;
			v2 *= n.y;
			v3 *= n.z;
			Ns[0] *= n.x;
			Ns[1] *= n.y;
			Ns[2] *= n.z;
			n = v1 + v2 + v3;
			
			m_stationaryPoints.push_back(n);
			v1 = n - m_toolPos.m_point;
			dot = DotProduct(v1, m_toolAxis);
			if(dot < 0.f) {
				m_stationaryPoints.push_back(m_toolPos.m_point);
				v1 *= -1.0;
			}
			else {
				m_stationaryPoints.push_back(m_toolAxis * dot + m_toolPos.m_point);
				v1 = m_toolAxis * dot + m_toolPos.m_point - n;
			}
			n = Normalized(Ns[0] + Ns[1] + Ns[2]);
			m_stationaryPoints.push_back(n);
			m_surfaceNormals.push_back(n);
			m_distances.push_back(m_radius - DotProduct(v1 ,n));
			////std::cout<<"===> dystans: " << m_distances.back() << std::endl;
		}
		++citer;
	}
}

void Core::MoveToolByMinimalReqDistance()
{
	if( !m_minimizer.MinTool(m_surfaceNormals, m_distances) ) return;
	m_toolPos.m_point += m_minimizer.Point();
}

void Core::NextStep()
{
	//m_path.TakeStep();
	ClearLists();
	FindTrianglesFacedToTool();
	FindStationaryPoints();
	if(!m_distances.empty()) 
		MoveToolByMinimalReqDistance();
	m_path.AddPath5CPoint(m_toolPos);
}

void Core::FillSpaceCubeRep()
{
	m_space.ReInit(m_model.GetBBox(), 10, 10, 10);
	m_space.Fill(m_model.GetMeshList());
	m_space.CompileSpace();
}

void Core::PrepareToFollowUp()
{
	FillSpaceCubeRep();
}

Box Core::BoundTool() const
{
	Box bbox;
	vector3 ax = m_toolAxis;
	ax *= m_model.GetBBox().GetMaxSize();
	ax += m_toolPos.m_point;
	bbox.m_min[0] = minf(m_toolPos.m_point.x, ax.x) - 1.5f * m_radius;
	bbox.m_min[1] = minf(m_toolPos.m_point.y, ax.y) - 1.5f * m_radius;
	bbox.m_min[2] = minf(m_toolPos.m_point.z, ax.z) - 1.5f * m_radius;

	bbox.m_max[0] = maxf(m_toolPos.m_point.x, ax.x) + 1.5f * m_radius;
	bbox.m_max[1] = maxf(m_toolPos.m_point.y, ax.y) + 1.5f * m_radius;
	bbox.m_max[2] = maxf(m_toolPos.m_point.z, ax.z) + 1.5f * m_radius;

	return bbox;
}

bool Core::CroutLUFactorization(const fmath::matrix33& a_A, fmath::matrix33& o_L, fmath::matrix33& o_U) const
{
	o_U.identity();
	o_L[0][0] = a_A[0][0];
	o_L[0][1] = a_A[0][1];
	o_L[0][2] = a_A[0][2];

	if(o_L[0][0] < EPS && o_L[0][0] > -EPS) return false;

	o_U[1][0] = a_A[1][0] / o_L[0][0];
	o_L[1][1] = a_A[1][1] - o_L[0][1] * o_U[1][0];
	o_L[1][2] = a_A[1][2] - o_L[0][2] * o_U[1][0];

	if(o_L[1][1] < EPS && o_L[1][1] > -EPS) return false;

	o_U[2][0] = a_A[2][0] / o_L[0][0];
	o_U[2][1] = (a_A[2][1] - o_L[0][1] * o_U[2][0])/o_L[1][1];
	o_L[2][2] = a_A[2][2] - o_L[0][2]*o_U[2][0] - o_L[1][2]*o_U[2][1];
	
	if(o_L[2][2] < EPS && o_L[2][2] > -EPS) return false;

	return true;
}

vector3 Core::Crout(const fmath::matrix33 &a_A, const fmath::vector3 &a_b) const
{
	matrix33 L(0.f), U(0.f);
	vector3 x;

	if( !CroutLUFactorization(a_A, L, U) ) 
		return vector3(0.f, 0.f, 0.f);
	x = Crout_Lw_Eq_b(L, a_b);
	x = Crout_Ux_Eq_w(U, x);
	return x;
}

bool Core::CroutTest() const
{
	matrix33 m;
	vector3 x, b, r;

	m.identity();
	b.set(2.f, 5.f, 9.f);
	r.set(2.f, 5.f, 9.f);
	//x = Crout(m, b);
	x = Gauss(m, b);
	if( !AreVectorsEqual(x, r) ) return false;

	m = matrix33(
		vector3(1.f, 5.f, 4.f), 
		vector3(2.f, -1.f, 3.f), 
		vector3(3.f, 0.f, 1.f)
		);
	b.set(10.f, 5.f, 7.f);
	r.set(1.f, 0.f, 3.f);
	//x = Crout(m, b);
	x = Gauss(m, b);
	if( !AreVectorsEqual(x, r) ) return false;

	m = matrix33(
		vector3(2.f, 3.f, 5.f), 
		vector3(0.f, 1.f, 6.f), 
		vector3(0.f, 0.f, 7.f)
		);
	b.set(0.f, .5f, 10.f);
	r.set(0.f, .5f, 1.f);
	//x = Crout(m, b);
	x = Gauss(m, b);
	if( !AreVectorsEqual(x, r) ) return false;

	m = matrix33(
		vector3(1.f, 3.f, 5.f), 
		vector3(0.f, 0.f, 0.f), 
		vector3(2.f, 4.f, 8.f)
		);
	b.set(5.f, 11.f, 21.f);
	r.set(1.f, 0.f, 2.f);
	x = Gauss(m, b);
	//x = Crout(m, b);
	if( !AreVectorsEqual(x, r) ) return false;

	return true;
}

vector3 Core::Gauss(const fmath::matrix33& a_A, const fmath::vector3& a_b) const
{
	unsigned int ind;
	float ftmp;
	vector3 b = a_b, tmp;
	matrix33 A = a_A;
	//A.transpose();

	//wybor max elementu w kolumnie
	if(fabs(A[1][0]) > fabs(A[2][0])) ind = 1;
	else ind = 2;
	if(fabs(A[0][0]) < fabs(A[ind][0])) {
		tmp = A[0];
		ftmp = b[0];
		A[0] = A[ind];
		b[0] = b[ind];
		A[ind] = tmp;
		b[ind] = ftmp;
	}
	//eliminacja
	b[0] /= A[0][0];
	A[0] /= A[0][0]; A[0][0] = 1.f;
	b[1] -= A[1][0] * b[0];
	A[1] -= A[1][0] * A[0];
	b[2] -= A[2][0] * b[0];
	A[2] -= A[2][0] * A[0];
	//wybor max elementu w kolumnie
	if(fabs(A[1][0]) < fabs(A[2][0])) {
		tmp = A[1];
		ftmp = b[1];
		A[1] = A[2];
		b[1] = b[2];
		A[2] = tmp;
		b[2] = ftmp;
	}
	//eliminacja
	b[1] /= A[1][1];
	A[1] /= A[1][1]; A[1][1] = 1.f;
	b[2] -= A[2][1] * b[1];
	A[2] -= A[2][1] * A[1];
	b[0] -= A[0][1] * b[1];
	A[0] -= A[0][1] * A[1];

	b[2] /= A[2][2]; A[2][2] = 1.f;
	b[1] -= A[1][2] * b[2];
	A[1] -= A[1][2] * A[2];
	b[0] -= A[0][2] * b[2];
	A[0] -= A[0][2] * A[2];

	return b;
}

void Core::CramerPrepareMatrix(const fmath::matrix33 &a_A, const fmath::vector3 &a_b, fmath::matrix33 &o_A) const
{
	o_A[0][0] = a_A[0][0] - a_A[2][0];
	o_A[1][0] = a_A[1][0] - a_A[2][0];
	o_A[2][0] = a_b[0] - a_A[2][0];

	o_A[0][1] = a_A[0][1] - a_A[2][1];
	o_A[1][1] = a_A[1][1] - a_A[2][1];
	o_A[2][1] = a_b[1] - a_A[2][1];

	o_A[0][2] = a_A[0][2] - a_A[2][2];
	o_A[1][2] = a_A[1][2] - a_A[2][2];
	o_A[2][2] = a_b[2] - a_A[2][2];
}

vector3 Core::Cramer(const fmath::matrix33 &a_A, const fmath::vector3 &a_b) const
{
	float W, Wx, Wy, W2, W3;
	matrix33 A;
	vector3 res(-1.f, -1.f, 0.f);

	CramerPrepareMatrix(a_A, a_b, A);
	W = A[0][0] * A[1][1] - A[1][0] * A[0][1];
	Wx = A[2][0] * A[1][1] - A[1][0] * A[2][1];
	Wy = A[0][0] * A[2][1] - A[2][0] * A[0][1];
	W2 = A[0][0] * A[1][2] - A[1][0] * A[0][2];
	W3 = A[0][1] * A[1][2] - A[1][1] * A[0][2];
	if(W3 > EPS || W3 < -EPS) {
		Wx = A[2][1] * A[1][2] - A[1][1] * A[2][2];
		Wy = A[0][1] * A[2][2] - A[2][1] * A[0][2];
		res.set(Wx/W3, Wy/W3, 0.f);	
	} else if(W2 > EPS || W2 < -EPS) {
		Wx = A[2][0] * A[1][2] - A[1][0] * A[2][2];
		Wy = A[0][0] * A[2][2] - A[2][0] * A[0][2];
		res.set(Wx/W2, Wy/W2, 0.f);	
	} else if(W > EPS || W < -EPS) {
		res.set(Wx/W, Wy/W, 0.f);	
	}
	/*else if( (Wx < EPS && Wx > -EPS) && (Wy < EPS && Wy > -EPS) ) {
		//nieskonczenie wiele rozw
		if(A[0][0] > EPS || A[0][0] < -EPS) {
			res.y = 0.f;
			res.x = A[2][0] / A[0][0];
		} else if(A[1][0] > EPS || A[1][0] < -EPS) {
			res.x = 0.f;
			res.y = A[2][0] / A[1][0];
		} else if(A[0][1] > EPS || A[0][1] < -EPS) {
			res.x = 0.f;
			res.y = A[2][1] / A[0][1];
		} else if(A[1][1] > EPS || A[1][1] < -EPS) {
			res.x = 0.f;
			res.y = A[2][1] / A[1][1];
		}
	}*/
	res.z = 1.0f - res.x - res.y;
	fmath::vector3 tmp = a_A * res;
	return res;
}

matrix33 Core::GetLocalTangentSpace(const fmath::vector3 &a_norm) const
{
	matrix33 axis;
	vector3 y(0.f, 1.f, 0.f);

	axis[2] = a_norm;
	if( AreVectorsEqual(axis[2], -y) ) y.set(0.f, 0.f, -1.f);
	if( AreVectorsEqual(axis[2], y) ) y.set(0.f, 0.f, 1.f);
	axis[0] = CrossProduct(a_norm, y);
	axis[1] = CrossProduct(axis[2], axis[0]);
	axis[0].normalize();
	axis[1].normalize();
	return axis;
}

void Core::CountError(const fmath::vector3 &a_p, const fmath::vector3 &a_n)
{
	float err = 0.f;
	vector3 toP = m_toolPos.m_point - a_p;
	toP = toP - DotProduct(toP, a_n) * a_n;
	err = toP.length();
	m_errorsMax = maxf(m_errorsMax, err);
	m_errorsMin = minf(m_errorsMin, err);
	m_errorsMean += err;
	++m_errorsNum;
}